#include "klt.h"


#define TESTE_MODE



/* Calcula as features que devem ser utilizadas. */
bool startKLT(IplImage *image_bin, CvRect rect, int *cont_feature, CvPoint2D32f *features){
	IplImage *imgaux = NULL, *imgaux2 = NULL;
	
	if ((rect.width > 0) && (rect.height > 0))
	{
		CvMat *submat = cvCreateMat(rect.width,rect.height,CV_8UC1);
		imgaux = cvCreateImage(cvGetSize(image_bin),IPL_DEPTH_32F,1);
		imgaux2 = cvCreateImage(cvGetSize(image_bin),IPL_DEPTH_32F,1);
		
		if ((!imgaux) || (!imgaux2) || (!submat)) 
		{
			return false;
		}

		cvGetSubRect(image_bin,submat,rect);
		cvGoodFeaturesToTrack(submat,imgaux,imgaux2,features,cont_feature,QUALITY,DISTANCE,NULL,BLOCK_SIZE,USE_HARRIS,HARRIS_K);

		cvReleaseImage(&imgaux);
		cvReleaseImage(&imgaux);
		cvReleaseMat(&submat);
		return true;
	}
	return false;
}

void kltTracking (IplImage *last, IplImage *current, CvPoint2D32f* old_features, CvPoint2D32f* new_features,
									int cont_features, char *status){
	//float track_error[cont_features];
	CvSize pyr_sz = cvSize( last->width+8, current->height/3 );
	IplImage* pyrA = cvCreateImage( pyr_sz, IPL_DEPTH_32F, 1 );
	IplImage* pyrB = cvCreateImage( pyr_sz, IPL_DEPTH_32F, 1 );

	cvCalcOpticalFlowPyrLK(last,current,pyrA,pyrB,old_features,new_features,cont_features,cvSize(WIN_SIZE,WIN_SIZE),
												 LEVEL,status,NULL,cvTermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS,20,0.3),0); 
												 
	cvReleaseImage(&pyrA);
	cvReleaseImage(&pyrB);
}
